// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#ifndef RM_SERIAL_DRIVER__PACKET_HPP_
#define RM_SERIAL_DRIVER__PACKET_HPP_

#include <algorithm>
#include <cstdint>
#include <vector>

namespace rm_serial_driver
{

//报文类型定义
enum PacketTypeEnum {
    ENUM_PACKET_HEART_BEAT = 0,
    ENUM_PACKET_SENTRY_ACCEPT_STATUS_DATA = 1,
    ENUM_PACKET_ARMOR_DATA = 2,
    ENUM_PACKET_SENTRY_SEND_CONTROL_DATA, //哨兵--装甲板信息
    ENUM_PACKET_GIMBAL_RESET,      //云台复位
    ENUM_PACKET_RADAR_SMALL_MAP,   //雷达发送的消息
    ENUM_PACKET_OBSTACLE_DATE,     //障碍快信息
    ENUM_PACKET_UNDEFINED,
    ENUM_PACKET_ARMORED_ACCEPT_STATUS_DATA,
    ENUM_PACKET_BUFF_DATA,
    ENUM_PACKET_SENTRY_ARMOR_DARA
};

enum ArmEnum{
    ENUM_ARM_SLAVE_COMPUTER = 0, //下位机，指电控
    ENUM_ARM_HERO = 1,
    ENUM_ARM_MINER = 2,
    ENUM_ARM_INFANTRY3 = 3, //3号步兵
    ENUM_ARM_INFANTRY4 = 4,//4号步兵
    ENUM_ARM_INFANTRY5 = 5,//5号步兵
    ENUM_ARM_UAV,
    ENUM_ARM_SENTRY,
    ENUM_ARM_DARTS,//飞镖
    ENUM_ARM_RADAR,//雷达
    ENUM_ARM_OPERATOR,//操作手
    ENUM_ARM_BASE,
    ENUM_ARM_OUTPOST,
    ENUM_ARM_UNDEFINED,//未定义兵种
    ENUM_ARM_ALL = 0xff
};

struct PacketHeader
{
    uint8_t start1 = 0xa5;
    uint8_t start2 = 0x5a;
    uint8_t from=0;
    uint8_t to=0;
    uint8_t packet_type=0;
    uint8_t data_len=0; //数据字段长度
    uint16_t checksum = 0;
} __attribute__((packed));

struct ArmoredAcceptStatusData
{
    uint8_t prior_armor_id = 1;
    float yaw_angle_fdb = 0;
    int16_t pitch_angle_fdb = 0;
    int16_t roll_angle_fdb = 0;
    int16_t yaw_angle_offset = 0;  // 未使用
    uint16_t bullet_speed = 0;
    int16_t pitch_angle_offset = 0;  // 未使用
    uint8_t color_of_me=0;
    uint8_t current_mode=0;
    uint32_t stm32_timestamp=0;
    uint8_t is_change=0;
    int8_t horizon_offset_count=0;
    int8_t verticle_offset_count=0;
} __attribute__((packed));

struct HeartBeatPacket {
    uint8_t color_of_me=0; // 当前我方的装甲板颜色,0表示红色，1表示蓝色
    uint8_t mode=0; // 需要上位机处于的模式
} __attribute__((packed));

struct ArmorDataPacket {
    uint16_t timestamp=0;
    uint16_t is_get=0; // 判断目标是否存在于视野中，0表示不存在，1表示存在
    uint16_t is_shoot=0;
    int16_t yaw=0;
    int16_t pitch=0;
    int16_t ui_x=0; // old xyz(now yaw pitch)
    int16_t ui_y=0; // old pitch
    int16_t _null2=0; // old vx
    int16_t _null3=0; // old vz
    int16_t _null4=0; // old armor id
    int16_t _null5=0; // old UI x
    int16_t _null6=0; // old UI y
} __attribute__((packed));

struct InfantrySendPackage{
  PacketHeader header;
  ArmorDataPacket armor_data_packet;
} ;

struct SendPacketVision
{
  uint8_t header = 0xA5;
  bool tracking : 1;
  uint8_t id : 3;          // 0-outpost 6-guard 7-base
  uint8_t armors_num : 3;  // 2-balance 3-outpost 4-normal
  uint8_t reserved : 1;
  float x;
  float y;
  float z;
  float yaw;
  float vx;
  float vy;
  float vz;
  float v_yaw;
  float r1;
  float r2;
  float dz;
  uint16_t checksum = 0;
} __attribute__((packed));

struct SendPacketTwist
{
  uint8_t header = 0xA4;
  float linear_x;
  float linear_y;
  float linear_z;
  float angular_x;
  float angular_y;
  float angular_z;
  uint16_t checksum = 0;
} __attribute__((packed));

template <typename T>
inline T fromVector(const std::vector<uint8_t> & data)
{
  T packet;
  std::copy(data.begin(), data.end(), reinterpret_cast<uint8_t *>(&packet));
  return packet;
}

template <typename T>
inline std::vector<uint8_t> toVector(const T & data)
{
  std::vector<uint8_t> packet(sizeof(T));
  std::copy(
    reinterpret_cast<const uint8_t *>(&data), reinterpret_cast<const uint8_t *>(&data) + sizeof(T),
    packet.begin());
  return packet;
}

static inline uint16_t calChecksum( const char *__data, const size_t __len )
{
    uint16_t my_checksum = 0;
    const char *ptr = __data;
    for ( int i = 0; i < (int)(__len / 2); i++ )
    {
        my_checksum += *( ( uint16_t * ) ptr );
        ptr += 2;
    }
    if ( __len & 0x01 )
    {
        uint16_t tmp = uint16_t( __data[ __len - 1 ] );
        my_checksum += tmp;
    }
    
    return my_checksum;
}


}  // namespace rm_serial_driver

#endif  // RM_SERIAL_DRIVER__PACKET_HPP_
